import math
class Turning:
    def __init__(self, config=None):
        # 存放数据的文件夹
        data_folder = config.get('data_folder')
        self.dis_mart = self.get("%s/Distance Matrix" % data_folder)
        self.path_mart = self.get("%s/Path Matrix" % data_folder)
        self.point_arr = self.get_pointarr("%s/Co-ordinate" % data_folder)

    def get(self, file_name):
        with open(file_name) as f1:
            list = f1.read()
            lines = list.split('\n')[:-1]
            mart = []
            for line in lines:
                m = line.split(' ')[:-1]
                mart.append([int(i) for i in m])
            return mart

    def get_pointarr(self, file_name):
        with open(file_name) as f1:
            list = f1.read()
            lines = list.split('\n')[:-1]
            mart = []
            for line in lines:
                m = {
                    "x": int(line.split(',')[0]),
                    "y": int(line.split(',')[1]),
                }
                mart.append(m)
            return mart

    def index_to_coord(self, index):
        return self.point_arr[index - 1]

    def path_get(self, start, target):
        path_list = []
        start -= 1
        target -= 1
        path_list.append(start + 1)
        temp = self.path_mart[start][target]
        while temp != target:
            path_list.append(temp + 1)
            temp = self.path_mart[temp][target]
        path_list.append(target + 1)
        return path_list

    def indexlist_to_arrlist(self, indexes):
        path_list = []
        for index in indexes:
            path_list.append(self.index_to_coord(index))
        return path_list

    def distance_get(self, start, target):
        return self.dis_mart[start - 1][target - 1]

    def culNextPoint(self, start, end, val=math.pi/6):
        # 速度为0.1s运动的像素数量,暂定为 1
        x_dif = end['x'] - start['x']
        y_dif = end['y'] - start['y']
        ang = math.atan2(y_dif, x_dif)
        # 测试转弯次数
        if self.ang is not None:
            if abs(ang - self.ang) > val:
                self.count += 1
                self.ang = ang
                return True
        self.ang = ang
        return False

    def modify_path(self, start, ori_ang, target, val=math.pi/6):
        l1 = self.distance_get(start, target)
        for index in range(len(self.point_arr)):
            if index != start & index != target:
                if len(path_planner.path_get(start, index)) == 2:
                    coord = self.index_to_coord(index)
                    coord_start = self.index_to_coord(start)
                    x_dif = coord['x'] - coord_start['x']
                    y_dif = coord['y'] - coord_start['y']
                    ang = math.atan2(y_dif, x_dif)
                    l2 = self.distance_get(start, index) + self.distance_get(index, target)
                    if (abs(ang - ori_ang) < val) and (l2 < (l1 * 1.1)):
                        self.path_mart[start - 1][target - 1] = index - 1
                        return True
        return False

def path_Print(start, end):
    print("v" + str(start) + "---v" + str(end) + "  Path: v" + str(start), end='')
    temp = path_planner.path_mart[start - 1][end - 1]
    while temp != end - 1:
        print("-->v" + str(temp + 1), end='')
        temp = path_planner.path_mart[temp][end - 1]
    print("-->v" + str(end), end='')
    print("")

def turning_count(start, end):
    index = path_planner.path_get(start, end)
    path = path_planner.indexlist_to_arrlist(index)
    path_planner.ang = None
    path_planner.count = 0
    turnings = []
    for i in range(len(path) - 1):
        ori_ang = path_planner.ang
        if(path_planner.culNextPoint(path[i], path[i + 1])):
            turnings.append((index[i], ori_ang))
    if path_planner.count > 2:
        path_Print(start, end)
        print("优化后：")
        for item in turnings:
            if path_planner.modify_path(item[0], item[1], end):
                path_Print(start, end)
                turning_count(start, end)
                return
    return

global path_planner
config_path = {
        'data_folder': './pathplanning',
    }
path_planner = Turning(config_path)
for i in range(1, 47):
    for j in range(1, 47):
        if i != j:
            turning_count(i, j)